#include "zf_common_headfile.h"

double A[6][6] = {   {0,1,0,0,0,0},
                     {0,0,-3.7,0,0,0},
                     {0,0,0,1,0,0},
                     {0,0,168.6,0,0,0},
                     {0,0,0,0,0,1},
                     {0,0,0,0,0,0}};
double B[6][2] = {{0,0},{0.428,0.428},{0,0},{-9.6,-9.6},{0,0},{-0.133,0.133}};
double Ke[2][6] = {{-0.7071,-15.8701,-122.1655,-28.3408,50.0000,5.8160},{-0.7071,-15.8701,-122.1655,-28.3408,-50.0000,-5.8160}};
double u_left=0,u_right=0;
double X[6] = {0};

extern LowPassFilter filter;
extern float angle_z;
void lqr_update(float location,float speed,float error)
{
    static float lowpass_output;
//    lowpass_output = lowpass_update(&filter,(motor_value.receive_right_speed_data+motor_value.receive_left_speed_data)/2);
    X[0] = location;
//    X[1] = 0;
    X[1] = -speed-lowpass_update(&filter,(motor_value.receive_right_speed_data+motor_value.receive_left_speed_data)/2);
    X[2] = euler_angle.roll;
    X[3] = imu660ra_gyro_x/16.384f;
    X[4] = error;
    X[5] = imu660ra_gyro_z/16.384f;
}
 void lqrControl(double x[6],double Kep[2][6],double output_limit)
{
     u_left = 0;u_right = 0;
        for(int j = 0;j<6;j++)
        {
            u_left += Kep[0][j]*x[j];
            u_right += Kep[1][j]*x[j];
        }
        if(u_left>output_limit){u_left=output_limit;}
        if(u_left<-output_limit){u_left=-output_limit;}
        if(u_right>output_limit){u_right=output_limit;}
        if(u_right<-output_limit){u_right=-output_limit;}
}
